/* (C) colornix 2024.
 *  send back too big ICMPV6 message.
 *
*/
/*
  Use libpcap to fetch raw video packets from C65GS, and then present them
  via a UDP socket for reading by the C65GS vncserver.  The idea is to
  separate the packet sniffer which needs root, from the part that listens
  to connections from the internet.

  (C) Paul Gardner-Stephen 2014, 2018.

  This program is free software; you can redistribute it and/or
  modify it under the terms of the GNU General Public License
  as published by the Free Software Foundation; either version 2
  of the License, or (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

*/

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <sys/socket.h>
// #include <sys/filio.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <linux/types.h>
#include <fcntl.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netinet/if_ether.h>
#include <netinet/tcp.h>
#include <netinet/ip.h>
#include <string.h>
#include <strings.h>
#include <signal.h>
#include <netdb.h>
#include <time.h>
#include <pcap.h>
#include <netinet/icmp6.h>
#include <netinet/ip6.h>

#define MAX_PACKET_SIZE 65536
int client_sock = -1;
int g_sock;
int g_mtu = 1280;
void print_pkt(char * packet)
{
        // 打印 Ethernet 头部信息
        const struct ether_header* eth = (struct ether_header*) packet;

        // 根据 EtherType 判断并解析上层协议
        switch (ntohs(eth->ether_type)) {
            case ETHERTYPE_IP:
                // IPv4
                //printf("IPv4 Packet...\n");
                //// TODO: 解析并打印 IPv4 头部及后续数据
                break;

            case ETHERTYPE_IPV6:
                // IPv6
        	printf("Ethernet Src: %02X:%02X:%02X:%02X:%02X:%02X\n",
               eth->ether_shost[0], eth->ether_shost[1], eth->ether_shost[2],
               eth->ether_shost[3], eth->ether_shost[4], eth->ether_shost[5]);
        	printf("Ethernet Dst: %02X:%02X:%02X:%02X:%02X:%02X\n",
               eth->ether_dhost[0], eth->ether_dhost[1], eth->ether_dhost[2],
               eth->ether_dhost[3], eth->ether_dhost[4], eth->ether_dhost[5]);
       		 printf("Ethernet Type: 0x%04X\n", ntohs(eth->ether_type));

                printf("IPv6 Packet...\n");
		printf("\n");
                // TODO: 解析并打印 IPv6 头部及后续数据
                break;

            case ETHERTYPE_ARP:
                // ARP
                //printf("ARP Packet...\n");
                // TODO: 解析并打印 ARP 头部及后续数据
                break;

            default:
                //printf("Unknown EtherType: 0x%04X\n", ntohs(eth->ether_type));
		break;
        }

        //printf("\n");

}
uint16_t in6_cksum(const struct icmp6_hdr *icmp6, const struct ip6_hdr *ip6, const uint8_t *data, size_t len) {
    uint32_t sum = 0;
    const uint16_t *ptr = (const uint16_t *)icmp6;
    int left = sizeof(struct icmp6_hdr) + sizeof(struct ip6_hdr) + len;

    // Add ICMPv6 header
    while (sizeof(struct icmp6_hdr) > 1 && left > 1) {
        sum += *ptr++;
        left -= 2;
    }

    // Add IP header
    ptr = (const uint16_t *)ip6;
    for (int i = 0; i < sizeof(struct ip6_hdr) / 2; ++i) {
        sum += *ptr++;
    }

    // Add data
    ptr = (const uint16_t *)data;
    for (int i = 0; i < len / 2; ++i) {
        sum += *ptr++;
    }

    // Add carry
    while (sum >> 16) {
        sum = (sum & 0xffff) + (sum >> 16);
    }

    return ~sum;
}

void process_ICMPv6(unsigned char* buffer, int size){
   struct icmp6_hdr *icmp;
   struct ip6_hdr *ip;
   u_int payload_len;
   u_int real_len;
   ip = (struct ip6_hdr *)(buffer + sizeof(struct ether_header));
   icmp = (struct icmp6_hdr *)(buffer + sizeof(struct ether_header) + sizeof(struct ip6_hdr));
   payload_len = ntohs(ip->ip6_plen);
   if (icmp->icmp6_type == ICMP6_ECHO_REQUEST) {
         printf("got icmp6 echo\n");
         if (payload_len > g_mtu) { // Example threshold for maximum payload size
		printf("payload too big : %d\n", payload_len);
                char xx[MAX_PACKET_SIZE];
                real_len = g_mtu - 40; // FIXME: should skip ip header which is 40 bytes.
                // Prepare the ICMPv6 error message
                memset(xx, 0, sizeof(xx));
                struct icmp6_hdr *icmp_reply = (struct icmp6_hdr *)xx;
                icmp_reply->icmp6_type = ICMP6_PACKET_TOO_BIG;
                icmp_reply->icmp6_code = 0;
                icmp_reply->icmp6_cksum = 0;
                int mtu = g_mtu;
                icmp_reply->icmp6_mtu = htonl(mtu); // Example MTU value
                memcpy(xx + sizeof(struct icmp6_hdr), ip, sizeof(struct ip6_hdr) + real_len); // Copy all bytes of echo request data, not exceed MTU

                // Calculate ICMPv6 checksum
                icmp_reply->icmp6_cksum = in6_cksum(icmp_reply, ip, (uint8_t *)(ip + sizeof(struct ip6_hdr)), g_mtu - 40 - 40 - 8);

                // 40 = ip6 header, 40 = lower ip6 header 8 = icmp header


                // Send the ICMPv6 error message
                struct sockaddr_in6 saddr = {
                        .sin6_family = AF_INET6, // 设置地址家族为 AF_INET6
                        .sin6_port = 0, // 设置端口号为 12345（网络字节序）
                        .sin6_addr = ip->ip6_src,
                        .sin6_scope_id = 0 // 如果需要指定接口ID（例如，在链接本地地址中），在这里设置
                };
                //hex_dump(xx, sizeof(struct icmp6_hdr) + sizeof(struct ip6_hdr) + real_len);
                if (sendto(g_sock, xx, g_mtu - 40, 0, (struct sockaddr *)&saddr, sizeof(saddr)) == -1) {
                    perror("sendto");
                }
         }
    }
}

void process_IPv6(unsigned char* buffer, int size)
{
    //ipv6++;
    struct ip6_hdr *ip6h = (struct ip6_hdr *)(buffer+ sizeof(struct ether_header));
    struct ether_header *eth = (struct ether_header*)(buffer);

    switch (ip6h->ip6_nxt)
    {
        case 6 :
            // printf("This is TCP inside IPv6\n");
            // process_TCP(buffer,size,0);

            break;
        case 17:
            // printf("This is UDP inside TPv6\n");
            //process_UDP(buffer,size,0);

            break;
        case 58:
           // printf("This is ICMPv6\n");
           process_ICMPv6(buffer, size);
        default:
            //printf("Print the IPv6 header\n" );

            //fprintf(logfile , "Packet type: IPv6\n");

            //print_IPV6_header((struct ipv6_header *)(buffer+ sizeof(struct ether_header)));
            // fprintf(logfile , "IPV6 Packet:\n");

            //printEther(eth);
            break;
    }
}

void process_Ether(unsigned char* buffer, int size)
{
        struct ether_header *eth = (struct ether_header*)(buffer);
    //Get the IP Header part of this packet , excluding the ethernet header
    //++total;
    switch (ntohs(eth->ether_type)) //Check the Protocol and do accordingly...
    {
        case 0x0800:  //IPv4 Protocol
            // process_IPv4(buffer,size);
                // printf("IPv4\n");
            // printEther(eth);

            break;

        case 0x0806:  //ARP Protocol
            // printEther(eth);
            // process_ARP(buffer,size);
                // printf("ARP\n");
            break;

        case 0x86dd:  //IPv6 Protocol
            process_IPv6(buffer,size);
                // printf("IPv6\n");
            // printEther(eth);

            break;

        default: //Some Other Protocol
            //            fprintf(logfile , "Packet type: Ethernet\n");

            //printEther(eth);
            break;
    }
    //printf("TCP : %d   UDP : %d   ICMP : %d   IGMP : %d   Others : %d   Total : %d\r", tcp , udp , icmp , igmp , others , total);
}

int main(int argc, char **argv)
{
  char *dev;
  char errbuf[PCAP_ERRBUF_SIZE];
  pcap_t *descr;
  //    struct bpf_program fp;        /* to hold compiled program */
  bpf_u_int32 pMask; /* subnet mask */
  bpf_u_int32 pNet;  /* ip address*/
  pcap_if_t *alldevs;

                 // Create a raw socket to listen for ICMPv6 packets
                if ((g_sock = socket(AF_INET6, SOCK_RAW, IPPROTO_ICMPV6)) == -1) {
                        perror("socket");
                        exit(EXIT_FAILURE);
                }

  // Prepare a list of all the devices
  if (pcap_findalldevs(&alldevs, errbuf) == -1) {
    fprintf(stderr, "Error in pcap_findalldevs: %s\n", errbuf);
    exit(1);
  }

  if (argv[1])
    dev = argv[1];
  else {
    fprintf(stderr, "You must specify the interface to listen on.\n");
    exit(-1);
  }

  // If something was not provided
  // return error.
  if (dev == NULL) {
    printf("\n[%s]\n", errbuf);
    return -1;
  }

  // fetch the network address and network mask
  pcap_lookupnet(dev, &pNet, &pMask, errbuf);

  // Now, open device for sniffing with big snaplen and
  // promiscuous mode enabled.
  descr = pcap_open_live(dev, 3000, 1, 10, errbuf);
  if (descr == NULL) {
    printf("pcap_open_live() failed due to [%s]\n", errbuf);
    return -1;
  }

  printf("Started.\n");
  fflush(stdout);


  while (1) {

    struct pcap_pkthdr hdr;
    hdr.caplen = 0;
    const unsigned char *packet = pcap_next(descr, &hdr);
    if (packet) {
      print_pkt(packet);
      process_Ether(packet , hdr.len);
    }
  }
  printf("Exiting.\n");

  return 0;
}
